classdef atrias
    %data analysis class for ATRIAS experiments
    %Version 3.1, May. 14, 2013
    
%V3 - no timing correction anymore

%call this class from inside the data folder
    
    %for planar data:  ^ 2 (y)          for 3D-data  ^ 2 (y)
  %                    |                             |  
  %                    |                             | 
  %                    |                             |
  %                    -------> 1 (x)                /---------> 1 (x)
  %                                                 /
  %                                                v 3 (z)
  % for ATRIAS  1 - lA, 2 - lB, 3 - rA, 4 - rB
    
  
%              O  z-Position
%             /|\ 
%           s/ |_\<- beta
%           /  |  \
%           \  |  /
%            \ | /
%             \|/    
  
  
    
  % conventions:
  
  %  - subcontroller numbering starts with '0'
  
  
    properties
        Version;
        Timing;         %time
        Kinematics;     %position velocity, angular position, angular velocity
        Dynamics;       %force, torque
        Electrics;      %voltage, current
        robotProperties;  %mass, gravitation, rest length, spring stiffness (leaf spring), segment length
        Analytics;      %selected samples, number of samples, data index (indicates missing samples in robot state), filter properties, trigger time (controller enable / force plate)
        COT_Data;       
        ControllerData;
    end
    
    methods
        function obj = atrias()                             %CONSTRUCTOR
        %% read files
            obj.Version.edition=2.0;                        %class Version
            obj.Version.date=date;                          %object creation date
            [filename, ~] = uigetfile('*.mat');             %select robot data from matfile
            M = importdata(filename, ' ');                  %write stored variables into struct
            if size(dir('*.txt'),1)>0                       %if forcefile exists
                [forcefile, ~] = uigetfile('*.txt');        %select forcefile
                if (isempty(regexp(forcefile,'^\d*', 'once')) && isempty(regexp(forcefile,'^DSADATA*', 'once')))
                    clear forcefile
                end
            end
            figure;                                         %display vertical robot position to select time range of interest
            plot(M.v_log__robot__state_zPosition(:, 1));
            [x, ~] = ginput(2);                             %read selection
            close
       %% find desired range of data for data and controller / correct for missing samples
            obj.Analytics.samples(1) = round(x(1));         %first data sample
            obj.Analytics.samples(2) = round(x(2));         %last data sample
            obj.Timing.Time = round(M.v_log__robot__state___time(obj.Analytics.samples(1))*1000):round(M.v_log__robot__state___time(obj.Analytics.samples(2))*1000); %create continuous timevector in ms
            obj.Analytics.lengthData = size(obj.Timing.Time,2);                 %define real length of trial in ms
            for idx=obj.Analytics.samples(1):obj.Analytics.samples(2)           %identify timing of samples
                obj.Analytics.DataIndex(idx-obj.Analytics.samples(1)+1)=find(round(M.v_log__robot__state___time(idx)*1000)==obj.Timing.Time,1);
            end
            if ~isempty(find(M.v_log__robot__state_rtOpsState==2,1))         %if trigger recorded
                obj.Analytics.Trigger=1;
            else
                obj.Analytics.Trigger=0;
            end
            obj.Analytics.TriggerTime = M.v_log__robot__state___time(find(M.v_log__robot__state_rtOpsState==2,1));
            obj.Analytics.TriggerOffset = (obj.Analytics.TriggerTime-M.v_log__robot__state___time(1))*1000;
            close

            %% import data
            a=['l','l','r','r'];                %order of data to be read
            b=['A','B','A','B'];

            %generate start and end time and equally spaced time vector
            obj.Timing.Start=obj.Timing.Time(1);
            obj.Timing.End=obj.Timing.Time(end);

             %% robot and analysis properties

            obj.robotProperties.m=60;
            obj.robotProperties.l0=0.90;
            obj.Analytics.FrequencyCutOff = 10;        %[Hz], cut-off frequency
            obj.Analytics.type = 'low';                %low pass filter, else high
            obj.Analytics.SamplingRate = 1000;         %[Hz], sampling frequency of the Medula
            obj.robotProperties.g=9.81;
            obj.robotProperties.krot=1600; %4118.3;    % [Nm/rad] rotatory spring stiffness, measured by Allison, for the largest spring
            obj.robotProperties.s=0.5;                 % [m] segment length
            obj.robotProperties.rcom=0.12;             % [m] Distance from leg pivot to center of mass
            obj.robotProperties.rboom=0.417;           % [m] Distance from leg pivot to boom pivot
            obj.robotProperties.boomh = 1.005;         % [m] boom base height
            obj.robotProperties.booml = 2.388;         % [m] boom length
            %%import robot state data
            %initialise fields
            index=obj.Analytics.DataIndex;
            obj.Kinematics.motors.MotorAngles=NaN(length(obj.Timing.Time),4);
            obj.Kinematics.motors.MotorVelocity=NaN(length(obj.Timing.Time),4);
            obj.Kinematics.motors.RotorAngles=NaN(length(obj.Timing.Time),4);
            obj.Kinematics.motors.RotorVelocity=NaN(length(obj.Timing.Time),4);
            obj.Kinematics.segmentAngles=NaN(length(obj.Timing.Time),4);
            obj.Kinematics.segmentVelocity=NaN(length(obj.Timing.Time),4);
            obj.Electrics.motorCurrent=NaN(length(obj.Timing.Time),4);
            obj.Electrics.motorVoltage=NaN(length(obj.Timing.Time),4);
            obj.Electrics.motorTherms=struct('lA',NaN(length(obj.Timing.Time),6),'lB',NaN(length(obj.Timing.Time),6),'rA',NaN(length(obj.Timing.Time),6),'rB',NaN(length(obj.Timing.Time),6));

            % Start and end indicies
            startN = obj.Analytics.samples(1);
            endN   = obj.Analytics.samples(2);

            % Hip data
            obj.Electrics.lHipMotorVoltage = M.v_log__robot__state_lHipMotorVoltage(startN:endN);
            obj.Electrics.rHipMotorVoltage = M.v_log__robot__state_rHipMotorVoltage(startN:endN);

            % For each leg segment
            for idx=1:4
                obj.Kinematics.motors.MotorAngles(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'MotorAngle(startN:endN)']);
                obj.Kinematics.motors.MotorVelocity(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'MotorVelocity(startN:endN)']);
                obj.Kinematics.motors.RotorAngles(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'RotorAngle(startN:endN)']);
                obj.Kinematics.motors.RotorVelocity(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'RotorVelocity(startN:endN)']);
                obj.Kinematics.segmentAngles(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'LegAngle(startN:endN)']);
                obj.Kinematics.segmentVelocity(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'LegVelocity(startN:endN)']);
                obj.Electrics.motorVoltage(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'MotorVoltage(startN:endN)']);
                if isfield(M,['v_log__robot__state_',a(idx),b(idx),'ClampedCmd'])
                    obj.Electrics.motorCurrent(index,idx)=eval(['M.v_log__robot__state_',a(idx),b(idx),'ClampedCmd(startN:endN)']);
                end
                for jdx=0:5
                    eval(['obj.Electrics.motorTherms.',a(idx),b(idx),'=M.v_log__robot__state_',a(idx),b(idx),'MotorTherm',num2str(jdx),'(startN:endN,:);']);
                end
            end
            obj.Kinematics.hipAngles=NaN(length(obj.Timing.Time),2);
            obj.Kinematics.hipAngles(index,:)=[M.v_log__robot__state_lLegBodyAngle(startN:endN) ...
                                               M.v_log__robot__state_rLegBodyAngle(startN:endN)];
            obj.Kinematics.hipVelocity=NaN(length(obj.Timing.Time),2);
            obj.Kinematics.hipVelocity(index,:)=[M.v_log__robot__state_lLegBodyVelocity(startN:endN) ...
                                                 M.v_log__robot__state_rLegBodyVelocity(startN:endN)];
            obj.Kinematics.Beta=[(obj.Kinematics.segmentAngles(:,2)-obj.Kinematics.segmentAngles(:,1))./2 ...
                                 (obj.Kinematics.segmentAngles(:,4)-obj.Kinematics.segmentAngles(:,3))./2];
            obj.Kinematics.legLength = 2 * obj.robotProperties.s .* cos(obj.Kinematics.Beta);
            obj.Kinematics.legAngles=[(obj.Kinematics.segmentAngles(:,2)+obj.Kinematics.segmentAngles(:,1))/2 ...
                                      (obj.Kinematics.segmentAngles(:,4)+obj.Kinematics.segmentAngles(:,3))/2];
            obj.Kinematics.position=NaN(length(obj.Timing.Time),2);
            obj.Kinematics.position(index,:)=[M.v_log__robot__state_xPosition(startN:endN)...
                                              M.v_log__robot__state_zPosition(startN:endN)];
            obj.Kinematics.boomposition=NaN(length(obj.Timing.Time),2);
            obj.Kinematics.boomposition=[M.v_log__robot__state_xPosition(startN:endN)...
                                              obj.robotProperties.boomh + obj.robotProperties.booml * sin(M.v_log__robot__state_boomAngle(startN:endN))];
            obj.Kinematics.velocity=NaN(length(obj.Timing.Time),2);
            obj.Kinematics.velocity(index,:)=[M.v_log__robot__state_xVelocity(startN:endN)...
                                              M.v_log__robot__state_zVelocity(startN:endN)];

            % Save torso angle and angular velocity
            obj.Kinematics.bodyPitch(index,:)=[M.v_log__robot__state_bodyPitch(startN:endN)...
                M.v_log__robot__state_bodyPitch(startN:endN)];
            obj.Kinematics.bodyPitchVelocity(index,:)=[M.v_log__robot__state_bodyPitchVelocity(startN:endN)...
                M.v_log__robot__state_bodyPitchVelocity(startN:endN)];
            obj.Kinematics.legPosition(index,:)=[obj.Kinematics.boomposition(index,1) - obj.robotProperties.rboom*sin(obj.Kinematics.bodyPitch(index,1) - 3*pi/2)... % x
                obj.Kinematics.boomposition(index,2) - obj.robotProperties.rboom*cos(obj.Kinematics.bodyPitch(index,2) - 3*pi/2)];  % y
            obj.Kinematics.legVelocity(index,:)=[obj.Kinematics.velocity(index,1) - obj.robotProperties.rboom*cos(obj.Kinematics.bodyPitch(index,1) - 3*pi/2).*obj.Kinematics.bodyPitchVelocity(index,1)... % dx
                obj.Kinematics.velocity(index,2) + obj.robotProperties.rboom*sin(obj.Kinematics.bodyPitch(index,2) - 3*pi/2).*obj.Kinematics.bodyPitchVelocity(index,2)];  % dy
        
            for idx=1:2
                obj.Kinematics.footpoint.x(idx,:)=obj.Kinematics.legLength(:,idx).*cos(obj.Kinematics.legAngles(:,idx));%-obj.Kinematics.position(:,1);
                obj.Kinematics.footpoint.y(idx,:)=-obj.Kinematics.legLength(:,idx).*sin(obj.Kinematics.legAngles(:,idx))+obj.Kinematics.position(:,2);
            end
            if isfield(M,'v_log__robot__state_currentPositive')
                obj.Electrics.measuredCurrent(:,2)=double(M.v_log__robot__state_currentNegative(startN:endN));
                obj.Electrics.measuredCurrent(:,1)=double(M.v_log__robot__state_currentPositive(startN:endN));
            else
                obj.Electrics.measuredCurrent(:,2)=double(M.v_log__robot__state_current600Amp(startN:endN));
                obj.Electrics.measuredCurrent(:,1)=double(M.v_log__robot__state_current50Amp(startN:endN));
            end
            obj.Kinematics.springDeflection=obj.Kinematics.motors.MotorAngles-obj.Kinematics.segmentAngles;

            %% parse atc_eq_point controller data
            load(filename);
            idx=1;
            contrnum=whos('*Controller__log_D');         %find number of pd-controllers used
            for idx=1:size(contrnum,1)
                contrname=contrnum(idx).name(1:strfind(contrnum(idx).name,num2str(idx-1)));    %determine flexible part of controller name
                start=eval(['find(round(M.',contrname,'Controller__log___time*1000)>=obj.Timing.Start,1)']);
                fini=eval(['find(round(M.',contrname,'Controller__log___time*1000)>obj.Timing.End,1)']);
                if isempty(fini)
                    fini=eval(['length(M.',contrname,'Controller__log___time)']);
                end
                index=eval(['M.',contrname,'Controller__log_header_seq(start:fini-1)-M.',contrname,'Controller__log_header_seq(start)+1']);
                obj.ControllerData.P(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.D(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.targetPos(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.targetVel(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.currentPos(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.currentVel(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.output(1:index(end),idx)=NaN(index(end),1);
                obj.ControllerData.P(index,idx)=eval(['M.',contrname,'Controller__log_P(start:fini-1)']);
                obj.ControllerData.D(index,idx)=eval(['M.',contrname,'Controller__log_D(start:fini-1)']);
                obj.ControllerData.targetPos(index,idx)=eval(['M.',contrname,'Controller__log_targetPos(start:fini-1)']);
                obj.ControllerData.targetVel(index,idx)=eval(['M.',contrname,'Controller__log_targetVel(start:fini-1)']);
                obj.ControllerData.currentPos(index,idx)=eval(['M.',contrname,'Controller__log_currentPos(start:fini-1)']);
                obj.ControllerData.currentVel(index,idx)=eval(['M.',contrname,'Controller__log_currentVel(start:fini-1)']);
                obj.ControllerData.output(index,idx)=eval(['M.',contrname,'Controller__log_output(start:fini-1)']);
            end

            % Save atc_slip_walking controller data
            if isfield(M, 'v_ATCSlipWalking__input_rvpp')
                obj.ControllerData.rvpp = M.v_ATCSlipWalking__input_rvpp;
                obj.ControllerData.qvpp = M.v_ATCSlipWalking__input_qvpp;
                obj.ControllerData.q1   = M.v_ATCSlipWalking__input_q1;
                obj.ControllerData.q2   = M.v_ATCSlipWalking__input_q2;
                obj.ControllerData.q3   = M.v_ATCSlipWalking__input_q3;
                obj.ControllerData.q4   = M.v_ATCSlipWalking__input_q4;
                obj.ControllerData.r0   = M.v_ATCSlipWalking__input_leg__length;

                % Chosen start and end robot state time indicies
                rsTime = M.v_log__robot__state___time;
                rsStartN = obj.Analytics.samples(1);
                rsEndN   = obj.Analytics.samples(2);
                rsTimeN = length(rsTime(rsStartN:rsEndN));
                % Preallocate as NaNs
                obj.ControllerData.controlFx(:,1:2) = NaN(rsTimeN,2);
                obj.ControllerData.controlFz(:,1:2) = NaN(rsTimeN,2);
                obj.ControllerData.computeFx(:,1:2) = NaN(rsTimeN,2);
                obj.ControllerData.computeFz(:,1:2) = NaN(rsTimeN,2);

                % Leg force subcontroller
                for idx = [1 2] % [left right]
                    if idx == 1 % left
                        contrname='v_ATCSlipWalking__ascLegForceL';
                    elseif idx == 2 % right
                        contrname='v_ATCSlipWalking__ascLegForceR';
                    end

                    % Get the time vector for this subcontroller
                    logTime = eval(['M.',contrname,'__log___time']);

                    % For all robot state times
                    for n = 1:rsTimeN
                        % The logTime index for this robot state time
                        logDataN = find(logTime == rsTime(rsStartN+n-1),1,'last');
                        % If this robot state time is in the log time
                        if ~isempty(logDataN)
                            % Store the control data
                            obj.ControllerData.controlFx(n,idx) = eval(['M.',contrname,'__log_control__fx(logDataN)']);
                            obj.ControllerData.controlFz(n,idx) = eval(['M.',contrname,'__log_control__fz(logDataN)']);
                            % Store the compute data
                            obj.ControllerData.computeFx(n,idx) = eval(['M.',contrname,'__log_compute__fx(logDataN)']);
                            obj.ControllerData.computeFz(n,idx) = eval(['M.',contrname,'__log_compute__fz(logDataN)']);
                        end
                    end
                end % for idx


                %{
                % Repurpose for debugging if necessary
                % Parse ATCSlipWalking variables
                contrname='v_ATCSlipWalking';
                % Get the time vector for this subcontroller
                logTime = eval(['M.',contrname,'__log___time']);
                % Preallocate as NaNs
                obj.ControllerData.q(:,1)           = NaN(rsTimeN,1);

                % For all robot state times
                for n = 1:rsTimeN
                    % The logTime index for this robot state time
                    logDataN = find(logTime == rsTime(rsStartN+n-1),1,'last');
                    % If this robot state time is in the log time
                    if ~isempty(logDataN)
                        % Store the control data
                        obj.ControllerData.q(n) = eval(['M.',contrname,'__log_q(logDataN)']);
                    end
                end % for n
                %}

            end % if isfield

            %% Phasing
            %parse phasing for ATCEqPoint if applicable
            if isfield(M,'v_ATCEqPoint__log_state')
                start=find(round(M.v_ATCEqPoint__log___time*1000)>=obj.Timing.Start,1);
                fini=find(round(M.v_ATCEqPoint__log___time*1000)>obj.Timing.End,1);
                if isempty(fini)
                    fini=length(M.v_ATCEqPoint__log___time);
                end
                index=M.v_ATCEqPoint__log_header_seq(start:fini-1)-M.v_ATCEqPoint__log_header_seq(start)+1;
                sensor=NaN(index(end),1);
                sensor(index)=M.v_ATCEqPoint__log_state(start:fini-1);
                phaseshift=diff(sensor);
                lr=find(phaseshift<0);   %index of transition l-r
                rl=find(phaseshift>1);  %index of transition r-l
                if numel(lr)>0 && numel(rl)>0
                    if rl(1)<lr(1)      %start with left-right transition
                        lr=[1;lr];
                    end
                    if rl(end)<lr(end)  %stop with right-left transition
                        rl=[rl;length(sensor)];
                    end
                    steps=(rl-lr)>50;                   %filter steps to be longer than 50ms
                    obj.Timing.stanceRight=[lr(steps) rl(steps)];
                    try
                        obj.Timing.stanceRight(:,3:4)=[obj.Timing.Time(lr(steps))' obj.Timing.Time(rl(steps))'];
                    catch
                        disp('step timing error!')
                        pause(1);
                    end
                end
                obj.Timing.Phases=M.v_ATCEqPoint__log_state;
                M.v_ATCEqPoint__log_state(round(M.v_ATCEqPoint__log_state)==11)=0.1;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==12)=0.2;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==13)=0.3;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==14)=0.4;
                M.v_ATCEqPoint__log_state(round(M.v_ATCEqPoint__log_state)==21)=0.6;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==22)=0.7;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==23)=0.8;
                M.v_ATCEqPoint__log_state(M.v_ATCEqPoint__log_state==24)=0.9;
                obj.Timing.phaseColor=M.v_ATCEqPoint__log_state(start:fini-1);
                obj.Timing.phaseShift=[1; find([0; diff(obj.Timing.phaseColor)]~=0); length(obj.Timing.phaseColor)];
                obj.Timing.phaseShift(:,2)=[obj.Timing.phaseColor(obj.Timing.phaseShift)];
            else
                % phase detection for ATCSLipWalking
                if isfield(M,'v_ATCSlipWalking__log_walkingState')
                    start=find(round(M.v_ATCSlipWalking__log___time*1000)>=obj.Timing.Start,1);
                    fini=find(round(M.v_ATCSlipWalking__log___time*1000)>obj.Timing.End,1);
                    if isempty(fini)
                        fini=length(M.v_ATCSlipWalking__log___time);
                    end
                    index=M.v_ATCSlipWalking__log_header_seq(start:fini-1)-M.v_ATCSlipWalking__log_header_seq(start)+1;
                    sensor=NaN(index(end),1);
                    if length(M.v_ATCSlipWalking__log_walkingState) > index
                        sensor(index)=M.v_ATCSlipWalking__log_walkingState(start:fini-1);
                        phaseshift=diff(sensor);
                        lr=find(phaseshift==3);   %index of transition l-r
                        rl=find(phaseshift==1);  %index of transition r-l
                        % Find takeoff and touchdown indicies
                        obj.Timing.ltd = find(phaseshift==1);
                        obj.Timing.rto = find(phaseshift==2);
                        obj.Timing.rtd = find(phaseshift==3);
                        obj.Timing.lto = find(phaseshift==-6);
                        if numel(lr)>0 && numel(rl)>0
                            if rl(1)<lr(1)      %start with left-right transition
                                lr=[1;lr];
                            end
                            if rl(end)<lr(end)  %stop with right-left transition
                                rl=[rl;length(sensor)];
                            end
                            steps=(rl-lr)>50;                   %filter steps to be longer than 50ms
                            obj.Timing.stanceRight=[lr(steps) rl(steps)];
                            try
                                obj.Timing.stanceRight(:,3:4)=[obj.Timing.Time(lr(steps))' obj.Timing.Time(rl(steps))'];
                            catch
                                disp('step timing error!')
                                pause(1);
                            end
                        end
                    end
                else
                    % for other controllers determine contact from spring
                    % deflection
                    % position - not accurate!!!
                    % 1 filter spring deflection
                    try     % in case toolbox not available
                        if isempty(find(isnan(obj.Kinematics.springDeflection),1))    % filter if no NANs in vector
                            [Bb,Ab] = butter(2,obj.Analytics.FrequencyCutOff/obj.Analytics.SamplingRate);
                            if sum(max(obj.Kinematics.springDeflection)>0.02)>1 %both legs in contact
                                fp = filtfilt(Bb,Ab,obj.Kinematics.springDeflection);
                            else
                                [~, Y]=max(obj.Kinematics.springDeflection);
                                [~,row]=max(Y);
                                obj.Analytics.leg = floor((row-1)/2)+1;     %save which leg the robot hopped on
                                fp = filtfilt(Bb,Ab,obj.Kinematics.springDeflection(isfinite(obj.Kinematics.springDeflection(:,row)),row));
                            end
                            contact = abs(fp) > 0.01;
                        else
                            %identify relevant spring, only one spring is
                            %considered -> programmed for hopping
                            [Bb,Ab] = butter(2,obj.Analytics.FrequencyCutOff/obj.Analytics.SamplingRate);
                            [Y, ~]=max(obj.Kinematics.springDeflection);
                            [~,row]=max(Y);
                            obj.Analytics.leg = floor((row-1)/2)+1;     %save which leg the robot hopped on
                            fp = filtfilt(Bb,Ab,obj.Kinematics.springDeflection(isfinite(obj.Kinematics.springDeflection(:,row)),row));
                            contact = abs(fp) > 0.01;
                        end
                    catch
                        fp = obj.Kinematics.springDeflection;
                        contact = abs(fp) > 0.02;
                    end
                    if size(contact,2)==4
                        phaseshift = [0 0 0 0;diff(contact)];
                        rl = find(phaseshift(:,1) == 1);
                        lr = find(phaseshift(:,3) == 1);
                        if numel(lr)>0 && numel(rl)>0
                            if rl(1)<lr(1)      %start with left-right transition
                                lr=[1;lr];
                            end
                            if rl(end)<lr(end)  %stop with right-left transition
                                rl=[rl;length(fp)];
                            end
                            steps=(rl-lr)>50;                   %filter steps to be longer than 50ms
                            obj.Timing.stanceRight=[lr(steps) rl(steps)];
                            try
                                obj.Timing.stanceRight(:,3:4)=[obj.Timing.Time(lr(steps))' obj.Timing.Time(rl(steps))'];
                            catch
                                disp('step timing error!')
                                pause(1);
                            end
                        end
                    else   % phasing for hopping
                        td = find(diff(contact) == 1);
                        to = find(diff(contact) == -1);
                        if td(1)>to(1)  %start with td
                            to = to(2:end);
                        end
                        l = min([length(to) length(td)]);
                        obj.Timing.stanceRight = [td(1:l) to(1:l)];
                    end
                end
            end
            
            
            %% calculate dynamics
            if exist('forcefile','var')
                obj = calcDynamics(obj,forcefile);
            else
                obj = calcDynamics(obj,'');
            end
        end
                
        function checkdata(obj)
            figure()
            subplot(2,1,1)
            plot(obj.Timing.Time,gradient(obj.Timing.Time))
            subplot(2,1,2)
            plot(obj.Timing.Time,obj.Kinematics.segmentAngles)
        end
        
        function plotPhases(obj,t)
            if isfield(obj.Timing,'stanceRight')
                gr=.9;
                h=gca;
                hold on
                pos=get(gca,'Position');
                a=ylim;
                b=xlim;
                axes('Position',pos);
                for idx=1:size(obj.Timing.stanceRight,1)
                    if t==0
                        patch([obj.Timing.stanceRight(idx,1:2) obj.Timing.stanceRight(idx,2:-1:1)],[a(1) a(1) a(2) a(2)],[gr gr gr],'EdgeColor',[gr gr gr]);
                    else
                        patch([obj.Timing.stanceRight(idx,3:4) obj.Timing.stanceRight(idx,4:-1:3)],[a(1) a(1) a(2) a(2)],[gr gr gr],'EdgeColor',[gr gr gr]);
                    end
                end
                set(gca,'xlim',b,'ylim',a);
                linkaxes([gca h],'xy');
                set(h,'Color','none')
                uistack(h,'top')
            end
        end
    
    
    end
    
    methods(Access = private)
        
        function obj = calcDynamics(obj,forcefile)
            obj.Dynamics.SpringTorque=obj.Kinematics.springDeflection.*obj.robotProperties.krot;
            % M/2/s/sin(beta) A: beta < 0, M > 0  B: beta > 0, M < 0
            obj.Dynamics.hipTorque = [sum(obj.Dynamics.SpringTorque(:,[1 2]),2) sum(obj.Dynamics.SpringTorque(:,[3 4]),2)];
            obj.Dynamics.axLegForce(:,1) = obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,1)/2/obj.robotProperties.s./sin(-obj.Kinematics.Beta(:,1))+...
                obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,2)/2/obj.robotProperties.s./sin(obj.Kinematics.Beta(:,1));
            obj.Dynamics.axLegForce(:,2) = obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,3)/2/obj.robotProperties.s./sin(-obj.Kinematics.Beta(:,2))+...
                obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,4)/2/obj.robotProperties.s./sin(obj.Kinematics.Beta(:,2));
            % M/2/s/sin(beta) A: beta < 0, M > 0  B: beta > 0, M < 0
            obj.Dynamics.tanLegForce(:,1) = obj.Dynamics.hipTorque(:,1)./obj.Kinematics.legLength(:,1);
            obj.Dynamics.tanLegForce_geom(:,1) = obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,1)/2/obj.robotProperties.s./cos(-obj.Kinematics.Beta(:,1))+...
                obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,2)/2/obj.robotProperties.s./cos(obj.Kinematics.Beta(:,1));
            obj.Dynamics.tanLegForce(:,2) = obj.Dynamics.hipTorque(:,2)./obj.Kinematics.legLength(:,2);
            obj.Dynamics.tanLegForce_geom(:,2) = obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,3)/2/obj.robotProperties.s./cos(-obj.Kinematics.Beta(:,2))+...
                obj.robotProperties.krot.*obj.Kinematics.springDeflection(:,4)/2/obj.robotProperties.s./cos(obj.Kinematics.Beta(:,2));
            for idx=1:2
                obj.Dynamics.Fx(:,idx)=-obj.Dynamics.axLegForce(:,idx).*cos(obj.Kinematics.legAngles(:,idx)+(obj.Kinematics.bodyPitch(:,idx)-3*pi/2))-obj.Dynamics.tanLegForce(:,idx).*sin(obj.Kinematics.legAngles(:,idx));
                obj.Dynamics.Fy(:,idx)= obj.Dynamics.axLegForce(:,idx).*sin(obj.Kinematics.legAngles(:,idx)+(obj.Kinematics.bodyPitch(:,idx)-3*pi/2))-obj.Dynamics.tanLegForce(:,idx).*cos(obj.Kinematics.legAngles(:,idx));
            end
            obj.Dynamics.Fx(:,3)=nansum(obj.Dynamics.Fx(:,1:2),2);
            obj.Dynamics.Fy(:,3)=nansum(obj.Dynamics.Fy(:,1:2),2);
            if ~isempty(forcefile)
                F=dlmread(forcefile,',');
                if obj.Analytics.Trigger
                    obj.Dynamics.FP.Fx=F(obj.Analytics.samples(1)-round(obj.Analytics.TriggerOffset):min(obj.Analytics.samples(2)-round(obj.Analytics.TriggerOffset),size(F,1)),1);
                    obj.Dynamics.FP.Fy=F(obj.Analytics.samples(1)-round(obj.Analytics.TriggerOffset):min(obj.Analytics.samples(2)-round(obj.Analytics.TriggerOffset),size(F,1)),3);
                    obj.Dynamics.FP.Fz=-F(obj.Analytics.samples(1)-round(obj.Analytics.TriggerOffset):min(obj.Analytics.samples(2)-round(obj.Analytics.TriggerOffset),size(F,1)),2);
                else
                    %treshold(1)=min(F(:,3));
                    %treshold(2)=min(obj.Dynamics.Fy(:,3));
                    riseFP=find(F(:,3)>10,1);
                    riseFy=find(obj.Dynamics.Fy(:,3)>10,1);
                    force1=riseFP-riseFy;
                    obj.Dynamics.FP.Fx=F(force1:min(force1+obj.Analytics.lengthData,length(F)),1);
                    obj.Dynamics.FP.Fy=F(force1:min(force1+obj.Analytics.lengthData,length(F)),3);
                    obj.Dynamics.FP.Fz=F(force1:min(force1+obj.Analytics.lengthData,length(F)),2);
                end
            end
        end
        
        
    end
end

